//
// Created by vivi on 26/04/2018.
//

#include <streambuf>
#include <istream>
#include "core/engine/andrewsellehe.h"
#include "testhairdata.h"
#include "testconfig.h"
#include <chrono>
#include <set>

std::ostream & operator<<(std::ostream & os, const HairEngine::AndrewSelleHairEngine::HairContactSpring & spring);
std::ostream & operator<<(std::ostream & os, const HairEngine::AndrewSelleHairEngine::Segment::Ptr & segmentIt);
std::ostream & operator<<(std::ostream & os, const std::set<HairEngine::AndrewSelleHairEngine::Segment::Ptr> & segmentPtrSet);

#include <gtest/gtest.h>

using namespace std;
using namespace Eigen;
using namespace HairEngine;

struct membuf : std::streambuf
{
	membuf(char* begin, char* end) {
		this->setg(begin, begin, end);
	}
};

ostream & operator<<(ostream & os, const AndrewSelleHairEngine::Segment::Ptr & segmentIt) {
	os << "Segment{id=" << segmentIt->globalIndex
	   << ",localIndex=" << segmentIt->localIndex()
	   << ",p1=" << EigenUtility::toString(segmentIt->p1->position)
	   << ",p2=" << EigenUtility::toString(segmentIt->p2->position)
	   << ",strandIndex=" << segmentIt->strandIndex()
	   << "}";
	return os;
}

ostream & operator<<(ostream & os, const AndrewSelleHairEngine::HairContactSpring & spring) {
	os << "HairContactSpring{l0=" << spring.l0
	   << ",k=" << spring.k
	   << ",l1={" << EigenUtility::toString(spring.l1->p1->position) << "," << EigenUtility::toString(spring.l1->p2->position) << "}"
	   << ",l2={" << EigenUtility::toString(spring.l2->p1->position) << "," << EigenUtility::toString(spring.l2->p2->position) << "}"
	   << ",t1=" << spring.t1
	   << ",t2=" << spring.t2
	   << ",l=" << ((spring.l1->p1->position + spring.t1 * (spring.l1->p2->position - spring.l1->p1->position)) - (spring.l2->p1->position + spring.t2 * (spring.l2->p2->position - spring.l2->p1->position))).norm();

	return os;
}

ostream & operator<<(ostream & os, const set<AndrewSelleHairEngine::Segment::Ptr> & segmentPtrSet) {
	os << "{ ";

	for (auto segmentIt : segmentPtrSet) {
		os << segmentIt << ",";
	}

	os << "}";

	return os;
}

TEST(AndrewSelleHairEngineTest, TestContactSpringsCreation) {

	const auto distanceComparator = [](const AndrewSelleHairEngine::SegmentComparatorStruct & comparator1, const AndrewSelleHairEngine::SegmentComparatorStruct & comparator2) -> bool {
#ifndef HAIRENGINE_TEST
		return comparator1.distanceSquared < comparator2.distanceSquared;
#else
		return comparator1.distanceSquared < comparator2.distanceSquared
		       || (comparator1.distanceSquared == comparator2.distanceSquared && comparator1.l2->globalIndex < comparator2.l2->globalIndex);
#endif
	};

	membuf buf((char*)Hair_hair, (char*)Hair_hair + Hair_hair_len);
	istream is(&buf);

	StrandBasedHairGeometry hairGeometry = StrandBasedHairGeometry(is);

	AndrewSelleHairEngine::Configuration hairConf;
	hairConf.hairContactsMaxContactPerSegment = 10;
	hairConf.hairContactsCreatingDistance = 5e-4f;
	hairConf.hairContactsBreakingDistance = 2e-3f;
	hairConf.hairContactsGridSize = 3e-4f;
	hairConf.externalAccleration = Vector3f(0.0f, 0.0f, 0.0f);
	hairConf.enableObjectCollision = false;
	hairConf.enableStrainLimiting = false;

	AndrewSelleHairEngine hairEngine(hairGeometry, hairConf, nullptr);

	HairEngineSimulationData data(Affine3f::Identity());
	data.update(Affine3f::Identity(), 0.03f);

	hairEngine.solveHairContacts(data);

	//Now all the hair spring contacts should be set
	vector<size_t> nContacts(hairEngine.segmentPtrs.size(), 0);

	for (auto l1 : hairEngine.segmentPtrs) {

		cout << "Checking: " << l1 << endl;

		//First check that the distance is right
		for (auto contactIt = l1->contactSpringBegin(); contactIt != l1->contactSpringEnd(); ++contactIt) {

			float l0Check, t1Check, t2Check;

			l0Check = std::sqrt(MathUtility::lineSegmentSquaredDistance(contactIt->l1->p1->position, contactIt->l1->p2->position,
					contactIt->l2->p1->position, contactIt->l2->p2->position, t1Check, t2Check));

			ASSERT_FLOAT_EQ(l0Check, contactIt->l0);
			ASSERT_FLOAT_EQ(t1Check, contactIt->t1);
			ASSERT_FLOAT_EQ(t2Check, contactIt->t2);
		}

		priority_queue<AndrewSelleHairEngine::SegmentComparatorStruct, vector<AndrewSelleHairEngine::SegmentComparatorStruct>, decltype(distanceComparator)> heap(distanceComparator);

		const size_t nNeeds = hairConf.hairContactsMaxContactPerSegment - nContacts[l1->globalIndex];
		for (auto l2 : hairEngine.segmentPtrs)
			if (l2->globalIndex > l1->globalIndex &&
			    l2->strandIndex() != l1->strandIndex() &&
			    nContacts[l2->globalIndex] < hairConf.hairContactsMaxContactPerSegment) {

			AndrewSelleHairEngine::SegmentComparatorStruct comparator;
			comparator.l2 = l2;
			comparator.distanceSquared = MathUtility::lineSegmentSquaredDistance(l1->p1->position, l1->p2->position, l2->p1->position, l2->p2->position, comparator.t1, comparator.t2);

			if (comparator.distanceSquared < hairConf.hairContactsCreatingDistance * hairConf.hairContactsCreatingDistance)
				heap.push(comparator);

			if (heap.size() > nNeeds)
				heap.pop();
		}

		size_t i = 0;

		while (!heap.empty()) {

			auto contactIt = l1->contactSpringBegin() + i;

			ASSERT_LE(contactIt, l1->contactSpringEnd());
			ASSERT_EQ(contactIt->l2, heap.top().l2);

			++nContacts[l1->globalIndex];
			++nContacts[heap.top().l2->globalIndex];

			heap.pop();

			++i;
		}
	}
}

